#include "RotateEuler.h"
#include <string.h>

// rotation about X axis
RotateEulerX::RotateEulerX(Dof *angle){
	mDofs.clear();
	mDofs.push_back(angle);
	strcpy(angle->getName(), "_eulerX");
	mDofs[0]->setTrans(this);
	mType = Transformation::T_ROTATEX;
}

RotateEulerX::RotateEulerX(Dof *angle, char *_name){
	mDofs.clear();
	mDofs.push_back(angle);
	strcpy(angle->getName(), "_eulerX");
	mDofs[0]->setTrans(this);
	mType = Transformation::T_ROTATEX;
	strcpy(mName, _name);
}

Mat4d *RotateEulerX::getTransformMat(){
	double cosangle = cos(mDofs[0]->getValue());
	double sinangle = sin(mDofs[0]->getValue());
	(*mTransform)[0][0] = 1.0; 
	(*mTransform)[1][1] = cosangle; 
	(*mTransform)[1][2] = -sinangle; 
	(*mTransform)[2][1] = sinangle; 
	(*mTransform)[2][2] = cosangle; 
	(*mTransform)[3][3] = 1.0; 
	return mTransform;
}

Mat4d *RotateEulerX::getDerivativeMat(Dof *d){
	if(d->getID()==mDofs[0]->getID()){
		double cosangle = cos(mDofs[0]->getValue());
		double sinangle = sin(mDofs[0]->getValue());
		(*mDeriv)[0][0] = 0.0; 
		(*mDeriv)[1][1] = -sinangle; 
		(*mDeriv)[1][2] = -cosangle; 
		(*mDeriv)[2][1] = cosangle; 
		(*mDeriv)[2][2] = -sinangle; 
		(*mDeriv)[3][3] = 0.0;
	}
	//else (*mDeriv)=vl_0;
	else {
		(*mDeriv)[0][0] = 0; 
		(*mDeriv)[1][1] = 0; 
		(*mDeriv)[1][2] = 0; 
		(*mDeriv)[2][1] = 0; 
		(*mDeriv)[2][2] = 0; 
		(*mDeriv)[3][3] = 0;
	}
	return mDeriv;
}

Mat4d *RotateEulerX::getDerivativeMat(Dof *d1, Dof *d2){
	assert(d1->getID()==d2->getID());	// shud only enter here in this case
	if(d1->getID()==mDofs[0]->getID()){
		double cosangle = cos(mDofs[0]->getValue());
		double sinangle = sin(mDofs[0]->getValue());
		(*mDeriv)[0][0] = 0.0; 
		(*mDeriv)[1][1] = -cosangle; 
		(*mDeriv)[1][2] = sinangle; 
		(*mDeriv)[2][1] = -sinangle; 
		(*mDeriv)[2][2] = -cosangle; 
		(*mDeriv)[3][3] = 0.0;
	}
	//else (*mDeriv)=vl_0;
	else {
		(*mDeriv)[0][0] = 0.0; 
		(*mDeriv)[1][1] = 0; 
		(*mDeriv)[1][2] = 0; 
		(*mDeriv)[2][1] = 0; 
		(*mDeriv)[2][2] = 0; 
		(*mDeriv)[3][3] = 0.0;
	}
	return mDeriv;
}

void RotateEulerX::applyGLTransform(){
	glRotatef(mDofs[0]->getValue()*180/M_PI, 1.0, 0.0, 0.0);
}

// rotation about Y axis
RotateEulerY::RotateEulerY(Dof *angle){
	mDofs.clear();
	mDofs.push_back(angle);
	strcpy(angle->getName(), "_eulerY");
	mDofs[0]->setTrans(this);
	mType = Transformation::T_ROTATEY;
}

RotateEulerY::RotateEulerY(Dof *angle, char *_name){
	mDofs.clear();
	mDofs.push_back(angle);
	strcpy(angle->getName(), "_eulerY");
	mDofs[0]->setTrans(this);
	mType = Transformation::T_ROTATEY;
	strcpy(mName, _name);
}

Mat4d *RotateEulerY::getTransformMat(){
	double cosangle = cos(mDofs[0]->getValue());
	double sinangle = sin(mDofs[0]->getValue());
	(*mTransform)[1][1] = 1.0; 
	(*mTransform)[2][2] = cosangle; 
	(*mTransform)[2][0] = -sinangle; 
	(*mTransform)[0][2] = sinangle; 
	(*mTransform)[0][0] = cosangle; 
	(*mTransform)[3][3] = 1.0; 
	return mTransform;
}

Mat4d *RotateEulerY::getDerivativeMat(Dof *d){
	if(d->getID()==mDofs[0]->getID()){
		double cosangle = cos(mDofs[0]->getValue());
		double sinangle = sin(mDofs[0]->getValue());
		(*mDeriv)[1][1] = 0.0; 
		(*mDeriv)[2][2] = -sinangle; 
		(*mDeriv)[2][0] = -cosangle; 
		(*mDeriv)[0][2] = cosangle; 
		(*mDeriv)[0][0] = -sinangle; 
		(*mDeriv)[3][3] = 0.0;
	}
	//else (*mDeriv)=vl_0;
	else {
		(*mDeriv)[1][1] = 0.0; 
		(*mDeriv)[2][2] = 0; 
		(*mDeriv)[2][0] = 0; 
		(*mDeriv)[0][2] = 0; 
		(*mDeriv)[0][0] = 0; 
		(*mDeriv)[3][3] = 0.0;
	}
	return mDeriv;
}

Mat4d *RotateEulerY::getDerivativeMat(Dof *d1, Dof *d2){
	assert(d1->getID()==d2->getID());	// shud only enter here in this case
	if(d1->getID()==mDofs[0]->getID()){
		double cosangle = cos(mDofs[0]->getValue());
		double sinangle = sin(mDofs[0]->getValue());
		(*mDeriv)[1][1] = 0.0; 
		(*mDeriv)[2][2] = -cosangle; 
		(*mDeriv)[2][0] = sinangle; 
		(*mDeriv)[0][2] = -sinangle; 
		(*mDeriv)[0][0] = -cosangle; 
		(*mDeriv)[3][3] = 0.0;
	}
	//else (*mDeriv)=vl_0;
	else {
		(*mDeriv)[1][1] = 0.0; 
		(*mDeriv)[2][2] = 0; 
		(*mDeriv)[2][0] = 0; 
		(*mDeriv)[0][2] = 0; 
		(*mDeriv)[0][0] = 0; 
		(*mDeriv)[3][3] = 0.0;
	}
	return mDeriv;
}

void RotateEulerY::applyGLTransform(){
	glRotatef(mDofs[0]->getValue()*180/M_PI, 0.0, 1.0, 0.0);
}

// rotation about Z axis
RotateEulerZ::RotateEulerZ(Dof *angle){
	mDofs.clear();
	mDofs.push_back(angle);
	strcpy(angle->getName(), "_eulerZ");
	mDofs[0]->setTrans(this);
	mType = Transformation::T_ROTATEZ;
}

RotateEulerZ::RotateEulerZ(Dof *angle, char *_name){
	mDofs.clear();
	mDofs.push_back(angle);
	strcpy(angle->getName(), "_eulerZ");
	mDofs[0]->setTrans(this);
	mType = Transformation::T_ROTATEZ;
	strcpy(mName, _name);
}

Mat4d *RotateEulerZ::getTransformMat(){
	double cosangle = cos(mDofs[0]->getValue());
	double sinangle = sin(mDofs[0]->getValue());
	(*mTransform)[2][2] = 1.0; 
	(*mTransform)[0][0] = cosangle; 
	(*mTransform)[0][1] = -sinangle; 
	(*mTransform)[1][0] = sinangle; 
	(*mTransform)[1][1] = cosangle; 
	(*mTransform)[3][3] = 1.0; 
	return mTransform;
}

Mat4d *RotateEulerZ::getDerivativeMat(Dof *d){
	if(d->getID()==mDofs[0]->getID()){
		double cosangle = cos(mDofs[0]->getValue());
		double sinangle = sin(mDofs[0]->getValue());
		(*mDeriv)[2][2] = 0.0; 
		(*mDeriv)[0][0] = -sinangle; 
		(*mDeriv)[0][1] = -cosangle; 
		(*mDeriv)[1][0] = cosangle; 
		(*mDeriv)[1][1] = -sinangle; 
		(*mDeriv)[3][3] = 0.0;
	}
	//else (*mDeriv)=vl_0;
	else {
		(*mDeriv)[2][2] = 0.0; 
		(*mDeriv)[0][0] = 0; 
		(*mDeriv)[0][1] = 0; 
		(*mDeriv)[1][0] = 0; 
		(*mDeriv)[1][1] = 0; 
		(*mDeriv)[3][3] = 0.0;
	}
	return mDeriv;
}

Mat4d *RotateEulerZ::getDerivativeMat(Dof *d1, Dof *d2){
	assert(d1->getID()==d2->getID());	// shud only enter here in this case
	if(d1->getID()==mDofs[0]->getID()){
		double cosangle = cos(mDofs[0]->getValue());
		double sinangle = sin(mDofs[0]->getValue());
		(*mDeriv)[2][2] = 0.0; 
		(*mDeriv)[0][0] = -cosangle; 
		(*mDeriv)[0][1] = sinangle; 
		(*mDeriv)[1][0] = -sinangle; 
		(*mDeriv)[1][1] = -cosangle; 
		(*mDeriv)[3][3] = 0.0;
	}
	//else (*mDeriv)=vl_0;
	else {
		(*mDeriv)[2][2] = 0.0; 
		(*mDeriv)[0][0] = 0; 
		(*mDeriv)[0][1] = 0; 
		(*mDeriv)[1][0] = 0; 
		(*mDeriv)[1][1] = 0; 
		(*mDeriv)[3][3] = 0.0;
	}
	return mDeriv;
}

void RotateEulerZ::applyGLTransform(){
	glRotatef(mDofs[0]->getValue()*180/M_PI, 0.0, 0.0, 1.0);
}
